/***************************************************************************
 创建者: 华磊
 开始时间: 2019.9.20
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) 姓名 2019.9.20 类的具体实现代码编写

 ***************************************************************************
 *  @file x.h
 *  功能:
 *
 *
 ***************************************************************************/
#ifndef PATHPLANMANAGER_H
#define PATHPLANMANAGER_H

#include <string>
#include <QString>
#include <QQueue>
#include "GeneralDefine.h"
#include <memory>
#include "messagelog.h"

class FleetTextCommandMaker;
class MapManager;
using MapManagerPtr = std::shared_ptr<MapManager>;
//class FleetService;

enum E_PLAN_POSITION_TYPE
{
    E_PLAN_POSITION_TYPE_COMMON,
    E_PLAN_POSITION_TYPE_VMARK,
    E_PLAN_POSITION_TYPE_QRCODE,
    E_PLAN_POSITION_TYPE_REFLECT,

};

class PathPlanManager
{
public:
    PathPlanManager(QString configFileIn);

public:
    int getCurrentFloorId();
    int getMapVersion();
    int getCurrentFleetMap( std::string &mapInfoReturn);
    int getFleetMap(std::string mapName, std::string &mapInfoReturn);//返回路径地图文本
    int writeFleetMap(std::string mapName, const string &mapInfoIn);
   int loadMap(std::string mapName);
   int setPositionFeedback(PositionInfo_hmi amcl,PositionInfo_hmi vmark,
                                PositionInfo_hmi qrcode, PositionInfo_hmi reflect);
   int setStationPathStatus(PathMovingInfo pathMovingInfoIn);

   int getBestPath_byPathInfo(PathMovingInfo pathMovingInfoIn, int targetStationId,
                              QStringList &pathCommandOut, int &tmpNearstOut, int &methodOut);
   int errorOccured();

private:
   int getBestPath_fromStationId(int startStationId, int targetStationId,
                   QStringList &pathCommandOut, int &tmpNearstOut);
   int getBestPath_fromPathId(int pathId, int targetStationId,
                                        QStringList &pathCommandOut);
   int isInPathRegion(int pathId, PositionInfo_hmi rosPose, PositionInfo_hmi reflectPose,
                      PositionInfo_hmi qrcodePose, PositionInfo_hmi vmarkPose,
                      double allowDistanceOffset, double allowAngleOffset);
   std::string getMapFleetFilePath(std::string mapNameIn);
   int getPathCommandString(int startStation ,int targetStationId, QStringList &pathCommandOut);
   int moveToNearstReportStation(E_PLAN_POSITION_TYPE poseType, QString &pathCommandOut,
                                                int &nearestIdout);
   int moveToPathEnd(int pathId, QString &pathCommandOut);
   int makeTransferStationPathCommand(int stationIdIn, QString &pathCommandOut);
   int getCommandStringByPathId(int pathId, bool isCnt, bool isForceLinePath, QString &pathCommandOut);
   double getTransAngleBiasOfSplines(int firstPathId, int secondPathId);
   int getRotatePrepareCommandByPathId( int pathId,QString &pathCommandOut);
   int readDefaultMapFilePath(QString configFilePathIn,QString &defaultMapPathOut);
   int initialConfigFile(QString configFileName);
   double angleRotationJudgement(double targetAngle,double initialAngle);//到目标姿态的最小路径转角

   void addMsg(int messageLevel,
                std::string componentName,
                std::string messageType,
                int messageCode,
                int robotId,
                QString parameter1 = "",
                int parameter2 = 0,
                int parameter3 = 0,
                int parameter4 = 0 );
private:
   MapManagerPtr  mapManager;
   FleetTextCommandMaker *commandMaker;
//   FleetService *fleetService;
   // 最后规划状态
   PositionInfo_hmi lastPlaned_position_amcl;//第一次规划在开始位置原地旋转到路径角度。后面记录目的站点的位置和到达角度。
   PositionInfo_hmi lastPlaned_position_vmark;
   PositionInfo_hmi lastPlaned_position_qrcode;
   PositionInfo_hmi lastPlaned_position_reflect;
   //not used
   int lastPlanedStationId;//最后一次规划的站点id,负数代表不存在。//todo 使用当前站点id代替.保证出错后也能正常规划，不去过渡点。
//   E_VEHICLE_POSITION_TYPE lastPlanedPoseType;NOT USED,通过添加开始角度参数，来解决是否需要旋转的问题。

   //当前实时状态
//   int passedStationId;//最后经过的站点id，负数代表未知
//   int nextStationId;//下一个站点id，负数代表未知
//   int currentStationId;//当前所在的站点id，负数表示不在站点
//   int passedPathId;//最后经过的路径id，负数代表未知
//   int nextPathId;//下一个路径id，负数代表未知
//   int currentPathId;//当前所处的路径id，负数表示不在任何路径
   PathMovingInfo pathMovingInfo;//not used
   //实时位置
   PositionInfo_hmi realtime_position_amcl;
   PositionInfo_hmi realtime_position_vmark;
   PositionInfo_hmi realtime_position_qrcode;
   PositionInfo_hmi realtime_position_reflect;

   bool isErrorOccured;
   bool isUsePathPlan;
   int pathPlanStratage;
   bool isInitialOkStatus;

   E_PLAN_POSITION_TYPE defaultPositionType;
   bool isUseDbMap;
   double cntAngleBias;
};

#endif // PATHPLANMANAGER_H
